static void __set_cursor_pos(int row, int col) 
{
	const int lcd_nr = row / row_per_ctrl;
	/* set the cursor position if need be.
	 * row, col - local variables, 
	 * disp_row, disp_rows, disp_col, disp_cols - global variables */
	if ( row > disp_rows || col > disp_cols ) return;
	if ( ( phcdev.lcd[lcd_nr].cur_x == col ) && ( phcdev.lcd[lcd_nr].cur_y == row ) ) return;
	if (test_bit(FLAG_CURSOR_ON, &phcdev.flags) && phcdev.cursor_on != lcd_nr) {
		hd44780_write_inst(phcdev.cursor_on,
					HD44780_DOOC_DISPLAYON |
					HD44780_DOOC_CURSOROFF |
					HD44780_DOOC_CURSORNOBLINK);
		hd44780_write_inst(lcd_nr,
					HD44780_DOOC_DISPLAYON |
					HD44780_DOOC_CURSORON |
					HD44780_DOOC_CURSORBLINK);
		phcdev.cursor_on = lcd_nr;
	}
	PDEBUG_IO(" row/col %d to %d %d ", row / row_per_ctrl, row, col);
	phcdev.cur_x = col;
	phcdev.cur_y = row;
	phcdev.lcd[lcd_nr].cur_x = col;
	phcdev.lcd[lcd_nr].cur_y = row%row_per_ctrl;
	hd44780_write_inst(row / row_per_ctrl,
			HD44780_DDRAM_ADDRES | ( ( row & 1 ) << 6 ) | col );
}

static void set_cursor_pos(int row, int col) 
{
	__set_cursor_pos(row,col);
}

static void increment_cursor_pos_on_operation(int row, int col)
{
	const int lcd_nr = row / row_per_ctrl;
	if (row%row_per_ctrl) {
		if (col < disp_cols) {
			phcdev.lcd[lcd_nr].cur_x++;
		} else {
			phcdev.lcd[lcd_nr].cur_x = 0;
			phcdev.lcd[lcd_nr].cur_y = 1;
		}
	} else {
		if (col < disp_cols) {
			phcdev.lcd[lcd_nr].cur_x++;
		} else {
			phcdev.lcd[lcd_nr].cur_x = 0;
			phcdev.lcd[lcd_nr].cur_y = 0;
		}
	}
}

/*static unsigned char readDDRAM(int row, int col)
{
	unsigned char buff;
	if ( row < 0 || col < 0 || row >= disp_rows || col >= disp_cols )
		return 0xff;
	PDEBUG_IO("rDDRAM: row:%d per_ctr:%d col:%d per_ctr:%d  stt: %#2x %1c",
		row,row_per_ctrl,col, ctrls_num, state(row, col), state(row, col));
	set_cursor_pos(row, col);
	buff = hd44780_read_data(row / row_per_ctrl);
	increment_cursor_pos(row, col);
	return buff;
}*/

static void writeCGRAM(unsigned int lcd_nr, unsigned int cgram_index, unsigned char *cgram_pixels) 
{
	/* write cgram to the specyfic controler on specified port */
	int i;
	PDEBUG_IO("wCGRAM: ctrl_nr:%d cgram_index:%d", ctrl_nr, cgram_index); 
	if (cgram_index > 7) return; 
	/* check if cgram chenged, no neeed to flush it, if no change */
	hd44780_write_inst(lcd_nr, 0x40 + ( 8 * cgram_index ) );
	for (i = 0; i < 8; i++ ) {
		phcdev.lcd[lcd_nr].cgram[cgram_index][i] = cgram_pixels[i];
		hd44780_write_data(lcd_nr, cgram_pixels[ i ] );
	}
	phcdev.lcd[lcd_nr].cur_x = -1;
	phcdev.lcd[lcd_nr].cur_y = -1;
}

static void writeDDRAM(unsigned char data, int row, int col)
{
	if ( row < 0 || col < 0 || row >= disp_rows || col >= disp_cols )
		return;
		
	PDEBUG_IO("wDDRAM: row:%d per_ctr:%d col:%d per_ctr:%d  data: %#2x %1c stt: %#2x %1c",
		row,row_per_ctrl,col, ctrls_num, data, data, state(row, col), state(row, col));
	/* when cursor is visible, need to move it no matter if writing smth or not */
	/* check if we really need to write anything */
	if (phcdev.state[row*disp_cols+col] == data ) {
		if (test_bit(FLAG_CURSOR_ON, &phcdev.flags))
			set_cursor_pos(row, col+1);
		return;
	}
	phcdev.state[row*disp_cols+col] = data;
	PDEBUG_IO(" changing ");
	/* when cursor is invisible, change its place only when actually writing to the device */
	set_cursor_pos(row, col);
	hd44780_write_data(row/row_per_ctrl, (unsigned char)data);
	increment_cursor_pos_on_operation(row, col);
}

static void clear_all_displays(void)
{
	hd44780_write_inst_all(HD44780_CLRDISP);
	memset(phcdev.state, charmap[' '], phcdev.screen_size*sizeof(*phcdev.state));
}

/* initialize all controllers */
static void init_displays (void)
{
	int i, j;
	port_init();
	hd44780_write_inst_all( HD44780_FS_DATAWIDTH8BIT |
				HD44780_FS_TWOLINES |
				HD44780_FS_5x8DOTS);
	hd44780_write_inst_all( HD44780_DOOC_DISPLAYON |
				HD44780_DOOC_CURSOROFF |
				HD44780_DOOC_CURSORNOBLINK);
	hd44780_write_inst_all( HD44780_CLRDISP);
	hd44780_write_inst_all( HD44780_EMS_INC | HD44780_EMS_NOSHIFT);
	/* initialization ends */
	
	/*
	 * write cgram if it is so defined in config.h, config.h should
	 * include specyfic file with table cg[8][8] and function init_charmap() 
	 * This writes all 8 entire (5x8) characters to the CGRAM from cg[i]
	 */
	PDEBUG_IO(" WRITING CGRAM \n");
	init_charmap();
	for (i = 0; i < ctrls_num; ++i) {
		for(j = 0; j < 8; ++j) {
#ifdef LOAD_CGRAM
			writeCGRAM(i, j, cg[j]);
#else
			writeCGRAM(i, j, "00000000");
#endif
		}
	}
	PDEBUG_IO("RET HOME\n");
	hd44780_write_inst_all(HD44780_RETURNHOME);
	/* set cursor on*/
	set_bit(FLAG_CURSOR_ON, &phcdev.flags);
	phcdev.cursor_on = 0;
	hd44780_write_inst(0,	HD44780_DOOC_DISPLAYON |
				HD44780_DOOC_CURSORON |
				HD44780_DOOC_CURSORBLINK);
}

